#include <iostream>
#include <opencv2/opencv.hpp>
#include <cmath>
#include <cstdlib>

using namespace cv;
using namespace std;

bool color_mode = true;

double dis(Point p1, Point p2){  //求两点距离
    double dis;
    dis = sqrt(pow(abs(p1.x - p2.x), 2) + pow(abs(p1.y - p2.y), 2));
    return dis;
}

int main(int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"usage: DisplayImage.out <Image_Path>\n";
        return -1;
    }
    Mat image;
    image = imread( argv[1], 1 );
    if ( !image.data )
    {
        cout<<"No image data \n";
        return -1;
    }
    namedWindow("Display Image", WINDOW_AUTOSIZE );
    cout<< "width: "<< image.cols <<"  "<<"high: "<< image.rows << " bite width: " << image.step <<endl;
    cout<< "Size: " << image.elemSize() << " " << "total: "  << image.total() << " " << "channles: " << image.channels() << endl; 
    imshow("Display Image", image);
    
    resize(image, image, Size(1800,1066));
    Mat img;
    image.copyTo(img);      //深拷贝（缺陷 较为消耗算力）

    int color;              //颜色模式判断
    if(color_mode == true)
        color = 0;          //蓝色模式
    else
        color = 2;          //红色模式      !!!阈值与蓝色有差别，需要重新调参

    for(int i = 0; i < image.cols - 1; i++){             //消除白色影响（非线性滤波）
        for(int j = 0; j < image.rows - 1; j++)
        {
            int b = image.at<Vec3b>(j,i)[0];
            int g = image.at<Vec3b>(j,i)[1];
            int r = image.at<Vec3b>(j,i)[2];
            if(b > 120 && g > 120 && r > 120)
            {
                image.at<Vec3b>(j,i)[0] = 0;
                image.at<Vec3b>(j,i)[1] = 0;
                image.at<Vec3b>(j,i)[2] = 0;
            }
        }
    }

    Mat result1;
    vector<Mat> channels;
    split(image, channels);     //通道分离
    Mat B_R = channels.at(color);  //提取颜色通道

    Mat th_b;               //二值化处理
    threshold(B_R, th_b, 210, 255, THRESH_BINARY);     //阈值给定考虑：180，210阈值在后续两种滤波之下会消除最小灯条轮廓

    Mat resultG;
    medianBlur(th_b, th_b, 11);                 //中值滤波
    GaussianBlur(th_b, resultG, Size(3,5), 3);  //高斯低通滤波
    Canny(resultG, resultG, 100, 200, 3);       //Canny算法

    //最小外接矩形 + 矩的计算
    vector<vector<Point>> contours;
    findContours(resultG, contours, 0, 2);  //轮廓点检测
    cout << "轮廓点数：" << contours.size() <<endl;

    for(int i = contours.size() - 1; i >= 0; i--){      //筛选单个符合几何条件矩形点位(对单个矩形限制：长宽比、角度)
        RotatedRect rrect = minAreaRect(contours[i]);
        Point2f points[4];
        rrect.points(points);
        Point2f cpt = rrect.center;

        if(((dis(points[0], points[1]) / dis(points[1], points[2])) > 2.0 || (dis(points[1], points[2]) / dis(points[1], points[0])) > 2.0 )    //长宽比条件限制
        && (rrect.angle > -25 || rrect.angle < -65)     //角度限制
         ){
            i -= 1;
            continue;
        }
            
        if(i >= 0){                     //pop出不符合条件点位
            auto it = contours.begin() + i;
            contours.erase(it);
        }
    }

    vector<RotatedRect> Lrect;     //定义灯条中心点vector

    for(int i = contours.size() - 1; i >= 0; i--){      //筛选灯条相关矩形点位(对多个矩形进行相关比较)
        RotatedRect rrect = minAreaRect(contours[i]);
        Point2f points[4];
        rrect.points(points);
        Point2f cpt = rrect.center;

        bool judge = false;  //条件判断标志位设立

        for(int j = contours.size() -1; j >= 0; j--){    //再次遍历contours，对coutours[i]进行相关性比较
            RotatedRect nrect = minAreaRect(contours[j]);
            Point2f pointn[4];
            nrect.points(pointn);
            Point2f cpn = nrect.center;

            Point2f cen((cpt.x + cpn.x)/2, (cpt.y + cpn.y)/2);  //取得二者的中心点
            int b = img.at<Vec3b>(cen)[0];
            int g = img.at<Vec3b>(cen)[1];
            int r = img.at<Vec3b>(cen)[2];

            if(i == j)      //排除自身的相关性
                continue;
            
            if((dis(cpt, cpn) < 50) && (b > 180 && g > 180 && r > 180)){    //条件判断
                circle(img, cen, 2, Scalar(0,0,255), 2, 8, 0);      //灯条中心点标记测试（red）
                judge = true;      //条件满足标志位赋值
                Lrect.push_back(rrect);    //中心点位存入
                Lrect.back().center = cen;
                break;
            }
        }

        if(judge == true)     //满足条件判断
            continue;

        if(i >= 0){         //移除不满足条件的矩形
            auto it = contours.begin() + i;
            contours.erase(it);
        }
    }

    cout << "轮廓点数：" << contours.size() <<endl;

    vector<RotatedRect> Boa;        //定义装甲板点位矩形变量
    
    for(int i = 0; i < Lrect.size(); i++){      //装甲板矩形获取

        for(int j = 0; j < Lrect.size(); j++){
            if(i == j)
                continue;

            Point2f cen((Lrect[i].center.x + Lrect[j].center.x)/2, (Lrect[i].center.y + Lrect[j].center.y)/2);  //取理论上亮灯条中心点
            int b = img.at<Vec3b>(cen)[0];
            int g = img.at<Vec3b>(cen)[1];
            int r = img.at<Vec3b>(cen)[2];

            if(abs(Lrect[i].angle - Lrect[j].angle) < 30 && abs(Lrect[i].center.y - Lrect[j].center.y) < 50){  //限制条件判断
                Boa.push_back(Lrect[i]);    //插入矩形
                Boa.back().center = cen;    //重新定义矩形中心点
                Boa.back().angle = (Lrect[i].angle + Lrect[j].angle) / 2;   //重新定义矩形角度
                Boa.back().size = Size(abs(Lrect[i].center.x - Lrect[j].center.x), abs(Lrect[i].center.x - Lrect[j].center.x) / 1.5);   //重新定义矩形规模
            }
        }
    }

    for(int i = Boa.size() - 1; i >= 0; i--){   //遍历Boa，装甲板矩形勾画
        Point2f points[4];
        Boa[i].points(points);
        Point2f cpt = Boa[i].center;

        for(int j = 0; j < 4; j++)          //框出最小矩形
        {
            if(j == 3)      //首尾相接
            {
                line(img, points[j], points[0], Scalar(255,0,255), 2, 8, 0);
                break;
            }
            line(img, points[j], points[j+1], Scalar(255,0,255), 2, 8, 0);    //接出三边
        }
        circle(img, cpt, 2, Scalar(255,0,255), 2, 8, 0);  //中心点标记   
    }

    imshow("result", img);

    //轮廓匹配

    waitKey(0);
    return 0;
}